/*
 *  Player Java Client 3 - HeadingControl.java
 *  Copyright (C) 2002-2006 Radu Bogdan Rusu, Maxim Batalin
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * $Id: HeadingControl.java 125 2011-03-24 02:24:05Z corot $
 *
 */
package javaclient3.extra;

import javaclient3.AbstractPositionDevice;
import javaclient3.PlayerClient;

/**
 * Heading control interface for Position, Position2D and Position3D Player
 * interfaces. Uses methods from both player interfaces and PIDController.
 * @author Radu Bogdan Rusu & Marius Borodi
 */
public class HeadingControl extends PIDController {

    private static final boolean isDebugging = PlayerClient.isDebugging;

    private AbstractPositionDevice device = null;

    /* PID coefficients */
    private int Kp = 1;
    private int Ki = 0;
    private int Kd = 0;

    private boolean stop = false;

    /* minimum and maximum admissible commands */
    private double minCommand = 1;
    private double maxCommand = 180;
    /* maximum allowed error */
    private double maxError   = 0;

    /**
     * Constructor for HeadingControl.
     * @param pd a reference to a PlayerDevice interface (Position, Position2D
     * or Position3D).
     */
    public HeadingControl (AbstractPositionDevice pd) {
        super (1, 0, 0);
        this.device = pd;
    }

    /**
     * Constructor for HeadingControl.
     * @param pd a reference to a PlayerDevice interface (Position, Position2D
     * or Position3D).
     * @param kp the proportional constant
     * @param ki the integral constant
     * @param kd the derivative constant
     */
    public HeadingControl (AbstractPositionDevice pd,
            int kp, int ki, int kd) {
        super (kp, ki, kd);
        this.Kp     = kp;
        this.Ki     = ki;
        this.Kd     = kd;
        this.device = pd;
    }

    /**
     * Constructor for HeadingControl.
     * @param pd a reference to a PlayerDevice interface (Position, Position2D
     * or Position3D).
     * @param minC minimum admissible command for the robot's motors
     * @param maxC maximum admissible command for the robot's motors
     */
    public HeadingControl (AbstractPositionDevice pd, int minC, int maxC) {
        super (1, 0, 0);
        this.minCommand = minC;
        this.maxCommand = maxC;
        this.device     = pd;
    }

    /**
     * Constructor for HeadingControl.
     * @param pd a reference to a PlayerDevice interface (Position, Position2D
     * or Position3D).
     * @param minC minimum admissible command for the robot's motors
     * @param maxC maximum admissible command for the robot's motors
     * @param kp the proportional constant
     * @param ki the integral constant
     * @param kd the derivative constant
     */
    public HeadingControl (AbstractPositionDevice pd, int minC, int maxC,
            int kp, int ki, int kd) {
        super (kp, ki, kd);
        this.minCommand = minC;
        this.maxCommand = maxC;
        this.Kp         = kp;
        this.Ki         = ki;
        this.Kd         = kd;
        this.device     = pd;
    }

    /**
     * Set the minimum admissible command for the robot's motors.
     * @param minC minimum admissible command as an integer
     */
    public void setMinimumCommand (double minC) {
        this.minCommand = minC;
    }

    /**
     * Set the maximum admissible command for the robot's motors.
     * @param maxC maximum admissible command as an integer
     */
    public void setMaximumCommand (double maxC) {
        this.maxCommand = maxC;
    }

    /**
     * Stop the robot from moving.
     */
    public void stopRobot () {
        this.stop = true;
    }

    /**
     * Set the maximum allowed error between the final goal and
     * the current position. (default error is 0)
     * @param err maximum allowed error as an integer
     */
    public void setAllowedError (double err) {
        this.maxError = err;
    }

    /**
     * Calculate and return the controller's command for the controlled system.
     * @param currentOutput the current output of the system
     * @return the new calculated command for the system
     */
    public double getCommand (double currentOutput) {
        this.currE = this.goal - currentOutput;

        /* Angle adjustments */
        if (currE  <= -180 )
            currE =  360 + currE;
        else
            if(currE >= 180  && currE <= 360)
                currE = currE - 360;
            else
                if(currE > 360)
                    currE = currE - 360;

        eSum += currE;

        lastE = currE;

        double Pgain = this.Kp * currE;
        double Igain = this.Ki * eSum;
        double Dgain = this.Kd * deltaE ();

        return Pgain + Igain + Dgain;
    }

    /**
     * Bound the output command to the minimum and maximum admissible commands.
     * @param command command to bound
     * @return new bounded command
     */
    private double boundCommand (double command) {
        if (command == 0)
            return 0;
        if (command < 0) {
            if (command > -minCommand)
                command = -minCommand;
            if (command < -maxCommand)
                command = -maxCommand;
        }
        else {
            if (command < minCommand)
                command = minCommand;
            if (command > maxCommand)
                command = maxCommand;
        }
        return command;
    }

    /**
     * Angle transformations, used internally.
     * @param angle angle to transform
     * @return new transformed angle
     */
    private double transformAngle (double angle) {
        angle = angle % 360;
        if (angle < 0)
            angle = 360 + angle;
        return angle;
    }

    /**
     * Rotate the robot on spot (differential heading) with a desired heading.
     * @param angle angle for rotation
     * @return false in case the rotation was interrupted, true otherwise
     */
    public boolean setDiffHeading (double angle) {
        if (angle == 0)
            return true;

        stop        = false;
        boolean ret = true;
        /* get the current heading */
        double currentHead = transformAngle (device.getYaw ());
        /* calculate the goal heading */
        double newGoal     = transformAngle (currentHead + angle);

        setGoal (newGoal);

        double now = transformAngle (device.getYaw ());

        /* keep rotating while the goal was not reached */
        while (now != newGoal) {
            if (stop == true) {
                ret = false;
                break;
            }

            /* no point in rotating at all if we're at +/-180 */
            if (Math.abs (now - newGoal) <= 1 && newGoal == 180)
                break;                  /* exit if we reached our destination */

            /* in case a diff. of maxError (default 0) between angles is acceptable */
            if (Math.abs (now - newGoal) <= maxError)
                break;                  /* exit if we reached our destination */

            /* get the current heading */
            now = transformAngle (device.getYaw ());

            /* get the motor command and check if within the desired limits */
            double command = getCommand (now);
            command = boundCommand (command);
            device.setSpeed (0, command);

            try { Thread.sleep (100); } catch (Exception e) {}
            if (isDebugging)
                System.err.println ("[HeadingControl][Debug] Angle left : " +
                        Math.abs (now - newGoal));
        }
        device.setSpeed (0, 0);         /* stop the robot from rotating */

        return ret;
    }

    /**
     * Rotate the robot on spot (absolute heading) to the desired heading.
     * @param angle goal angle
     * @return false in case the rotation was interrupted, true otherwise
     */
    public boolean setHeading (double angle) {
        /* get the current heading */
        double currentAngle =  transformAngle (device.getYaw ());

        /* difference between the current heading and the goal heading */
        double deltaAngle   = (angle - currentAngle);

        if (deltaAngle != 0) {
            if (deltaAngle <= 180 && deltaAngle > 0)
                return setDiffHeading (deltaAngle);
            else
                if (deltaAngle > -180)
                    return setDiffHeading (-360 + deltaAngle);
                else
                    return setDiffHeading (360 + deltaAngle);
        }
        return true;
    }
}
